#include "StdAfx.h"
#include "OffGridNavigationManager.h"

#include "SmartObjects.h"
#include "NavigationSystem.h"


OffMeshNavigationManager::OffMeshNavigationManager(const int offMeshMapSize)
: m_offMeshMap(offMeshMapSize)
, m_objectRegistrationEnabled(false)
{

}

const MNM::OffMeshNavigation& OffMeshNavigationManager::GetOffMeshNavigationForMesh( const NavigationMeshID& meshID ) const
{
	if (meshID && m_offMeshMap.validate(meshID))
	{
		return m_offMeshMap[meshID];
	}
	else
	{
		assert(0);
		AIWarning("No off-mesh navigation available, returning empty one! Navigation might not be able to use external links when path-finding");
		return m_emptyOffMeshNavigation;
	}
}

MNM::OffMeshNavigation& OffMeshNavigationManager::GetOffMeshNavigationForMesh( const NavigationMeshID& meshID )
{
	if (meshID && m_offMeshMap.validate(meshID))
	{
		return m_offMeshMap[meshID];
	}
	else
	{
		assert(0);
		AIWarning("No off-mesh navigation available, returning empty one! Navigation might not be able to use external links when path-finding");
		return m_emptyOffMeshNavigation;
	}
}

//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
class CRegisterSOHelper
{
public:
	struct SHelperInfo
	{
		SHelperInfo(const MNM::TriangleID& _triangleID, const Vec3& _position, SmartObjectHelper* _pHelper)
			: triangleID(_triangleID)
			, position(_position)
			, pHelper(_pHelper)
		{

		}

		SmartObjectHelper*				pHelper;
		MNM::TriangleID					triangleID;
		Vec3							position;
	};
	typedef std::vector<SHelperInfo> THelpersInfo;

	struct SHelpersLink
	{
		SHelpersLink(int from, int to)
			: fromIndex(from)
			, toIndex(to)
		{

		}

		bool operator==(const SHelpersLink& other) const
		{
			return (fromIndex == other.fromIndex) && (toIndex == other.toIndex);
		}

		int fromIndex;
		int toIndex;
	};
	typedef std::vector<SHelpersLink>	TObjectLinks;

	struct SAgentTypeInfo
	{
		friend class CRegisterSOHelper;

		SAgentTypeInfo(NavigationAgentTypeID _agentTypeID)
			: agentTypeID(_agentTypeID)
		{

		}

		ILINE NavigationAgentTypeID GetAgentTypeID() const { return agentTypeID; }
		
		bool IsUserClassCompatible(const char* userClass)
		{
			for (size_t i = 0; i < userClasses.size(); ++i)
			{
				if (strcmp(userClasses[i].c_str(), userClass))
					continue;

				return true;
			}

			return false;
		}

	private:

		ILINE void AddUserClass(const char* userClass) { stl::push_back_unique(userClasses, userClass); }

		NavigationAgentTypeID	agentTypeID;
		std::vector<string>		userClasses;	
	};

	CRegisterSOHelper( CSmartObject& smartObject, CSmartObjectClass& smartObjectClass)
		: m_smartObject(smartObject)
		, m_smartObjectClass(smartObjectClass)
		, m_navigationSystem(*gAIEnv.pNavigationSystem)
	{
		GenerateCompatibleTypes();	
	}

	bool GetAgentInfo(SAgentTypeInfo& agentInfo)
	{
		if (m_currentElement < m_supportedAgentTypes.size())
		{
			agentInfo = m_supportedAgentTypes[m_currentElement];
			m_currentElement++;
			return true;
		}

		return false;
	}

	bool CanRegister() const
	{
		return !(m_supportedAgentTypes.empty());
	}

private:

	void GenerateCompatibleTypes()
	{
		const CSmartObjectClass::THelperLinks::const_iterator itLinksBegin = m_smartObjectClass.m_vHelperLinks.begin();
		const CSmartObjectClass::THelperLinks::const_iterator itLinksEnd = m_smartObjectClass.m_vHelperLinks.end();

		size_t agentTypeCount = m_navigationSystem.GetAgentTypeCount();
		m_supportedAgentTypes.reserve(agentTypeCount);	

		for (size_t agentIdx = 0; agentIdx < agentTypeCount; ++agentIdx)
		{
			const NavigationAgentTypeID agentTypeId = m_navigationSystem.GetAgentTypeID(agentIdx);
			bool addAgentTypeInfo = false;
			SAgentTypeInfo agentTypeInfo(agentTypeId);

			for(CSmartObjectClass::THelperLinks::const_iterator linkIt = itLinksBegin; linkIt != itLinksEnd; ++linkIt)
			{
				const CSmartObjectClass::HelperLink& helperLink = *linkIt;
				const char* userClassName = (helperLink.condition && helperLink.condition->pUserClass) ? helperLink.condition->pUserClass->GetName().c_str() : NULL;

				if (userClassName && m_navigationSystem.AgentTypeSupportSmartObjectUserClass(agentTypeId, userClassName))
				{
					agentTypeInfo.AddUserClass(userClassName);
					addAgentTypeInfo = true;
				}
			}			

			if (addAgentTypeInfo)
			{
				m_supportedAgentTypes.push_back(agentTypeInfo);
			}
		}	

		m_currentElement = 0;
	}

	typedef std::vector<SAgentTypeInfo>	TSupportedAgentTypes;
	TSupportedAgentTypes	m_supportedAgentTypes;
	size_t					m_currentElement;

	CSmartObject&		m_smartObject;
	CSmartObjectClass&	m_smartObjectClass;
	NavigationSystem&	m_navigationSystem;
};

void OffMeshNavigationManager::RegisterSmartObject( CSmartObject* pSmartObject, CSmartObjectClass* pSmartObjectClass )
{
	assert(pSmartObject && pSmartObjectClass);

	if (!CanRegisterObject() || pSmartObjectClass->IsSmartObjectUser())
		return;

	//Filter out non-compatible smart-objects first...
	CRegisterSOHelper registerSOHelper(*pSmartObject, *pSmartObjectClass);
	if (!registerSOHelper.CanRegister())
		return;

	if (ObjectRegistered(pSmartObject->GetEntityId(), pSmartObjectClass->GetName()))
	{
		UnregisterSmartObject(pSmartObject, pSmartObjectClass->GetName());
	}

	//////////////////////////////////////////////////////////////////////////
	/// Register the object, no matter what, so we keep track of it
	TRegisteredObjects::iterator registeredObjectIt = m_registeredObjects.find(pSmartObject->GetEntityId());
	if (registeredObjectIt == m_registeredObjects.end())
	{
		std::pair<TRegisteredObjects::iterator, bool> newObjectIt = m_registeredObjects.insert(TRegisteredObjects::value_type(pSmartObject->GetEntityId(), TSOClassInfo()));
		assert(newObjectIt.second);

		registeredObjectIt = newObjectIt.first;
	}

	//////////////////////////////////////////////////////////////////////////
	/// Iterate through all compatible agent types for the smart object
	CRegisterSOHelper::SAgentTypeInfo agentTypeInfo(NavigationAgentTypeID(0));
	CRegisterSOHelper::THelpersInfo soHelpersInfo;
	soHelpersInfo.reserve(pSmartObjectClass->m_vHelperLinks.size());

	while(registerSOHelper.GetAgentInfo(agentTypeInfo))
	{
		NavigationAgentTypeID agentTypeID = agentTypeInfo.GetAgentTypeID(); 

		NavigationMeshID targetMeshID;
		soHelpersInfo.clear();
		soHelpersInfo.reserve(pSmartObjectClass->m_vHelperLinks.size());

		//////////////////////////////////////////////////////////////////////////
		/// Find out to which mesh the object should belong
		for (CSmartObjectClass::SetHelpers::iterator itHelpersEnd = pSmartObjectClass->m_setNavHelpers.end(),
			itHelpers = pSmartObjectClass->m_setNavHelpers.begin(); itHelpers != itHelpersEnd; ++itHelpers)
		{
			SmartObjectHelper* pHelper = *itHelpers;

			// Calculate position
			const Vec3 helperPosition = pSmartObject->GetHelperPos(pHelper);

			NavigationMeshID meshID = gAIEnv.pNavigationSystem->GetEnclosingMeshID(agentTypeID , helperPosition);

			if (targetMeshID && (targetMeshID != meshID))
			{
				//Bail out, not all helpers are connected to the same mesh
				return;
			}

			targetMeshID = meshID;
			soHelpersInfo.push_back(CRegisterSOHelper::SHelperInfo(0, helperPosition, pHelper));	
		}

		//////////////////////////////////////////////////////////////////////////
		/// Object belongs to a mesh...
		if (targetMeshID)
		{
			SObjectMeshInfo objectMeshInfo(targetMeshID);

			//////////////////////////////////////////////////////////////////////////
			/// Make sure all helpers are connected to some triangle before creating any link
			NavigationMesh& navigationMesh = gAIEnv.pNavigationSystem->GetMesh(targetMeshID);
			const MNM::real_t range = MNM::real_t(1.0f);

			for (CRegisterSOHelper::THelpersInfo::iterator helperIt = soHelpersInfo.begin(); helperIt != soHelpersInfo.end(); ++helperIt)
			{
				const MNM::vector3_t testPosition = MNM::vector3_t(MNM::real_t(helperIt->position.x), MNM::real_t(helperIt->position.y), MNM::real_t(helperIt->position.z));
				const MNM::TriangleID triangleID = navigationMesh.grid.GetTriangleAt(testPosition, range, range);

				if (!triangleID)
				{
					//Bail out, not all helpers are connected to a triangle
					return;
				}

				helperIt->triangleID = triangleID;
			}

			//////////////////////////////////////////////////////////////////////////
			/// Select the corresponding off-mesh navigation object, and finalize the links
			assert(m_offMeshMap.validate(targetMeshID));
			MNM::OffMeshNavigation& offMeshNavigation = m_offMeshMap[targetMeshID];

			const CSmartObjectClass::THelperLinks::const_iterator itLinksBegin = pSmartObjectClass->m_vHelperLinks.begin();
			const CSmartObjectClass::THelperLinks::const_iterator itLinksEnd = pSmartObjectClass->m_vHelperLinks.end();

			CRegisterSOHelper::TObjectLinks registeredLinks;
			registeredLinks.reserve(pSmartObjectClass->m_vHelperLinks.size());

			const int linkIndex = 0;
			for(CSmartObjectClass::THelperLinks::const_iterator linkIt = itLinksBegin; linkIt != itLinksEnd; ++linkIt)
			{
				const CSmartObjectClass::HelperLink& helperLink = *linkIt;
				const char* userClassName = (helperLink.condition && helperLink.condition->pUserClass) ? helperLink.condition->pUserClass->GetName().c_str() : NULL;

				if (!userClassName || !agentTypeInfo.IsUserClassCompatible(userClassName))
					continue;

				int helperIndex = 0;
				int fromIndex = -1;
				int toIndex = -1;
				for (CSmartObjectClass::SetHelpers::iterator itHelpersEnd = pSmartObjectClass->m_setNavHelpers.end(),
					itHelpers = pSmartObjectClass->m_setNavHelpers.begin(); itHelpers != itHelpersEnd; ++itHelpers)
				{
					if (linkIt->from == *itHelpers)
					{
						fromIndex = helperIndex;
					}
					else if (linkIt->to	== *itHelpers)
					{
						toIndex = helperIndex;
					}
					helperIndex++;
				}

				if ((fromIndex != -1) && (toIndex != -1))
				{
					const bool alreadyRegistered = stl::find(registeredLinks, CRegisterSOHelper::SHelpersLink(fromIndex, toIndex));

					if (!alreadyRegistered)
					{
						OffMeshLink_SmartObject linkData(pSmartObject->GetEntityId(), pSmartObject, pSmartObjectClass, 
							soHelpersInfo[fromIndex].pHelper, soHelpersInfo[toIndex].pHelper);
						offMeshNavigation.AddLink(navigationMesh, soHelpersInfo[fromIndex].triangleID, soHelpersInfo[toIndex].triangleID, linkData);
						
						stl::push_back_unique(objectMeshInfo.triangleList, soHelpersInfo[fromIndex].triangleID);
						
						registeredLinks.push_back(CRegisterSOHelper::SHelpersLink(fromIndex, toIndex));
					}
				}
			}
			uint32 smartObjectClassNameCrC = gEnv->pSystem->GetCrc32Gen()->GetCRC32Lowercase(pSmartObjectClass->GetName().c_str());
			registeredObjectIt->second[smartObjectClassNameCrC].push_back(objectMeshInfo);
		}
	}
}

void OffMeshNavigationManager::UnregisterSmartObjectForAllClasses( CSmartObject* pSmartObject )
{
	CSmartObjectClasses& classes = pSmartObject->GetClasses();
	CSmartObjectClasses::iterator it, itEnd = classes.end();

	for ( it = classes.begin(); it != itEnd; ++it )
	{
		UnregisterSmartObject(pSmartObject, (*it)->GetName());
	}
}

void OffMeshNavigationManager::UnregisterSmartObject( CSmartObject* pSmartObject, const string& smartObjectClassName )
{
	assert(pSmartObject);

	const EntityId objectId = pSmartObject->GetEntityId();
	TRegisteredObjects::iterator objectIt = m_registeredObjects.find(objectId);

	if (objectIt != m_registeredObjects.end())
	{
		uint32 smartObjectClassNameCrC = gEnv->pSystem->GetCrc32Gen()->GetCRC32Lowercase(smartObjectClassName.c_str());
		TSOClassInfo::iterator classIt = objectIt->second.find(smartObjectClassNameCrC);
		if(classIt != objectIt->second.end())
		{
			const TObjectInfo& objectInfo = classIt->second;
			for (TObjectInfo::const_iterator objectMeshInfoIt = objectInfo.begin(); objectMeshInfoIt != objectInfo.end(); ++objectMeshInfoIt)
			{
				const NavigationMeshID meshID = objectMeshInfoIt->meshID;
				if (m_offMeshMap.validate(meshID))
				{
					NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
					MNM::OffMeshNavigation& offMeshNavigation = m_offMeshMap[meshID];

					const SObjectMeshInfo::TTriangleList& triangleList = objectMeshInfoIt->triangleList;

					for(SObjectMeshInfo::TTriangleList::const_iterator triangleCit = triangleList.begin(); triangleCit != triangleList.end(); ++triangleCit)
					{
						offMeshNavigation.RemoveLink(mesh, *triangleCit, objectId);
					}
				}
			}
			objectIt->second.erase(classIt);
		}
		if(objectIt->second.size() == 0)
			m_registeredObjects.erase(objectIt);
	}
}

void OffMeshNavigationManager::RefreshConnections( const NavigationMeshID meshID, const MNM::TileID tileID )
{
	if (m_offMeshMap.validate(meshID))
	{
		MNM::OffMeshNavigation& offMeshNavigation = m_offMeshMap[meshID];

		//////////////////////////////////////////////////////////////////////////
		// Invalidate links for the tile. 
		// Because the tile has been regenerated, all off-mesh links are invalid now
		offMeshNavigation.InvalidateLinks(tileID);

		std::vector<EntityId> tempObjectIds;
		tempObjectIds.reserve(m_registeredObjects.size());

		//////////////////////////////////////////////////////////////////////////
		// Find objects which need to be registered again because of the change
		// - Objects with no connections, the mesh modification could have allowed them to connect now
		// - Objects with connections in this mesh-tile, need to re-check they are valid
		for (TRegisteredObjects::const_iterator objectIt = m_registeredObjects.begin(); objectIt != m_registeredObjects.end(); ++objectIt)
		{
			const TSOClassInfo classInfos = objectIt->second;

			for(TSOClassInfo::const_iterator classIt = classInfos.begin(); classIt != classInfos.end(); ++classIt)
			{
				const TObjectInfo& objectInfo = classIt->second;
				const EntityId objectId = objectIt->first;
			
				const bool connectedToMeshes = !objectInfo.empty();
				if (connectedToMeshes)
				{
					for (TObjectInfo::const_iterator objectMeshInfoIt = objectInfo.begin(); objectMeshInfoIt != objectInfo.end(); ++objectMeshInfoIt)
					{
						// Find out if object is bound to the modified mesh and tile
						const SObjectMeshInfo& objectMeshInfo = *objectMeshInfoIt;
						if (objectMeshInfo.meshID == meshID)
						{
							for (SObjectMeshInfo::TTriangleList::const_iterator triangleIt = objectMeshInfo.triangleList.begin(); triangleIt != objectMeshInfo.triangleList.end(); ++triangleIt)
							{
								if (tileID == MNM::ComputeTileID(*triangleIt))
								{
									tempObjectIds.push_back(objectId);
									break;
								}
							}
						}
					}
				}
				else
				{
					tempObjectIds.push_back(objectId);
				}
			}
		}

		//////////////////////////////////////////////////////////////////////////
		/// Register again those objects which could have been affected by the change
		for (std::vector<EntityId>::const_iterator objectIt = tempObjectIds.begin(); objectIt != tempObjectIds.end(); ++objectIt)
		{
			CSmartObject* pSmartObject = CSmartObjectManager::GetSmartObject(*objectIt);
			assert(pSmartObject);
			if (pSmartObject)
			{
				CSmartObjectClasses& classes = pSmartObject->GetClasses();
				CSmartObjectClasses::iterator it, itEnd = classes.end();
				for ( it = classes.begin(); it != itEnd; ++it )
				{
					CSmartObjectClass* pSmartObjectClass = *it;

					RegisterSmartObject( pSmartObject, pSmartObjectClass );
				}
			}
		}

	}
}

void OffMeshNavigationManager::Clear()
{
	m_offMeshMap.clear();
	m_registeredObjects.clear();

	m_objectRegistrationEnabled = false;
}

void OffMeshNavigationManager::Enable()
{
	m_objectRegistrationEnabled = true;
}

void OffMeshNavigationManager::OnNavigationMeshCreated( const NavigationMeshID& meshID )
{
	assert(!m_offMeshMap.validate(meshID));

	m_offMeshMap.insert(meshID, MNM::OffMeshNavigation());
}

void OffMeshNavigationManager::OnNavigationMeshDestroyed( const NavigationMeshID& meshID )
{
	assert (m_offMeshMap.validate(meshID));
	
	m_offMeshMap.erase(meshID);

	for (TRegisteredObjects::iterator objectIt = m_registeredObjects.begin(); objectIt != m_registeredObjects.end(); ++objectIt)
	{
		for(TSOClassInfo::iterator classInfoIt = objectIt->second.begin(); classInfoIt != objectIt->second.end(); ++classInfoIt)
		{
			TObjectInfo& objectInfo = classInfoIt->second;
			stl::find_and_erase(objectInfo, meshID);
		}

	}
}

void OffMeshNavigationManager::OnNavigationLoadedComplete()
{
	//////////////////////////////////////////////////////////////////////////
	//Only after the navigation loaded process is completed off-mesh links can be created
	//Off-mesh links are created after the mesh is loaded, they are not stored within the mesh when exported
	Enable();
}

bool OffMeshNavigationManager::ObjectRegistered( const EntityId objectId, const string& smartObjectClassName ) const
{
	uint32 smartObjectClassNameCrC = gEnv->pSystem->GetCrc32Gen()->GetCRC32Lowercase(smartObjectClassName.c_str());
	TRegisteredObjects::const_iterator itFoundObject = m_registeredObjects.find(objectId);
	return (itFoundObject != m_registeredObjects.end() && itFoundObject->second.find(smartObjectClassNameCrC) != itFoundObject->second.end());
}

bool OffMeshNavigationManager::IsObjectLinkedWithNavigationMesh(const EntityId objectId) const
{
	TRegisteredObjects::const_iterator objectIt = m_registeredObjects.find(objectId);

	return (objectIt != m_registeredObjects.end()) ? !objectIt->second.empty() : false;
}

//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

#if DEBUG_MNM_ENABLED

void OffMeshNavigationManager::UpdateEditorDebugHelpers( )
{
	if (!gEnv->IsEditing())
		return;

	static float time = 0.0f;

	time += (gEnv->pTimer->GetFrameTime() * 2.5f);

	IRenderAuxGeom* pRenderAux = gEnv->pRenderer->GetIRenderAuxGeom();

	SAuxGeomRenderFlags oldFlags = pRenderAux->GetRenderFlags();
	SAuxGeomRenderFlags renderFlags(oldFlags);

	renderFlags.SetAlphaBlendMode(e_AlphaBlended);
	renderFlags.SetDepthWriteFlag(e_DepthWriteOff);

	pRenderAux->SetRenderFlags(renderFlags);

	TRegisteredObjects::const_iterator objectIt = m_registeredObjects.begin();
	TRegisteredObjects::const_iterator endIt = m_registeredObjects.end();

	const float alpha = clamp((1.0f + sinf(time)) * 0.5f, 0.25f, 0.7f);
	const ColorB color (255, 0, 0, (uint8)(alpha * 255));

	for( ; objectIt != endIt; ++objectIt)
	{
		if (!objectIt->second.empty())
			continue;

		IEntity* pObjectEntity = gEnv->pEntitySystem->GetEntity(objectIt->first);
		if (pObjectEntity)
		{
			IStatObj** pStaticObjects = NULL;
			gAIEnv.pSmartObjectManager->GetSOClassTemplateIStatObj(pObjectEntity, pStaticObjects);
			if (pStaticObjects)
			{
				pRenderAux->DrawAABB(pStaticObjects[0]->GetAABB(), pObjectEntity->GetWorldTM(), true, color, eBBD_Faceted);
			}
		}
		
	}

	pRenderAux->SetRenderFlags(oldFlags);
}

#endif